#include <joyos.h>
#include <happylib.h>
#include <math.h>

#include "define.h"
#include "monitor.h"
#include "distance.h"
#include "roboturn.h"
#include "robomove.h"
#include "bumper.h"
#include "fantasy.h"

struct lock mon_lock;
int pos[3] = {0,0,0};
int posNow[2] = {0,0};
int8_t place = 0;

void gameMonitor(){
	int (*monitorpt)(void) = monitor;
	init_lock (&mon_lock, "Mon lock");
	create_thread(monitorpt, 1000, 150, "Monitor");
}

int monitor(void){
	while(1){
		if(status == 0){ //stop or scoring
			yield();
		}else if(status == 1){
			//updatePos();
			if(stuck() == 1 || stuck() == -1){
				check_stuck = 1;
			}else{
				check_stuck = 0;
			}
		}else if(status == 2){
			//updateDegree();
			if(stuck() == 1 || stuck() == -1){
				check_stuck = 2;
			}else{
				check_stuck = 0;
			}
		}else if(status == 3){
			//updatePosBack();
			if(stuck() == 1 || stuck() == -1){
				check_stuck = 3;
			}else{
				check_stuck = 0;
			}
		}
		//printf("\n x:%d %d y:%d deg:%d", pos[0], pos[1], pos[2], posNow[0]);//--> already pause in stuck()
	}
	return 0;
}


int shaftToDistance(int16_t shaft){ // inch
	return DIST_PER_WHEEL/SHAFT_PER_WHEEL*shaft;
}
void setPosNow(){
	posNow[0] = pos[0];
	posNow[1] = pos[1];
}
// Start Pos = (0,0,0) (x,y,deg)
void updatePos(){ //Rely on shaft encoder
	uint16_t nowL = encoder_read(ENCODER_PORT_LEFT);
	uint16_t nowR = encoder_read(ENCODER_PORT_RIGHT);
	int delta_dist = (shaftToDistance(nowL-shaftValueMonitorL) + shaftToDistance(nowR-shaftValueMonitorR))/2;
	pos[0] = (delta_dist*cos(pos[2]));
	pos[1] = (delta_dist*sin(pos[2]));
}
void updatePosBack(){ //Rely on shaft encoder
	uint16_t nowL = encoder_read(ENCODER_PORT_LEFT);
	uint16_t nowR = encoder_read(ENCODER_PORT_RIGHT);
	int delta_dist = (shaftToDistance(nowL-shaftValueMonitorL) + shaftToDistance(nowR-shaftValueMonitorR))/2;
	pos[0] = -(delta_dist*cos(pos[2]));
	pos[1] = -(delta_dist*sin(pos[2]));
}

void updateDegree(){ //Rely on gyro
	pos[2] = getCurrentAngle();
}

int *getRobotPos(){ //[0] = x [1] =y [2] = degree
	return pos;
}
/*
 * @param place
 * 0 = LEFT HOME
 * 1 = Go to 1 
 * 2 = LEFT 1
 * 3 = Go to 2
 * 4 = LEFT 2
 * 5 = Go to 4
 * 6 = LEFT 4 
 * 7 = Go to SKUNK
 * 8 = SKUNK
 * 9 = RIGHT 4
 * 10 = Go to Right 2
 * 11 = RiIGHT 2
 * 12 = Go to Right 4
 * 13 = RIGHT 4
 * 14 = Go to Right 1
 * 15 = RIGHT HOME
 */
void moveTo(int place){
	
}
void resolveUnfulfillState(){
	//myStrategy();
}

bool foundEnemy(void){
//	double threshold = 10.0;
//	double enemyDistance = getDistance();
//	if (enemyDistance <= threshold)
//		return true;
//	else
//		return false;
	
	return false;
}

int8_t stuck(void){

	uint16_t current_threshold = 900;	
	uint16_t currentLEFT = motor_get_current_MA(MOTOR_PORT_LEFT);
	uint16_t currentRIGHT = motor_get_current_MA(MOTOR_PORT_RIGHT);	
	//printf("\nCurrent L %d, R %d", currentLEFT, currentRIGHT);
	int8_t countL = 0;
	int8_t countR = 0;
	for(int i=0; i< 10 ;i++){
		if(currentLEFT > current_threshold)
			countL ++;	
		if(currentRIGHT > current_threshold)
			countR ++;
	}
	if (countL > 7){
		return 1;
	}else if (countR > 7){
		return -1;
	}else{
		return 0;
	}	
} 



int8_t stuckMotor(void){
	uint16_t current_threshold = 1000;	
	uint16_t currentLEFT = motor_get_current_MA(MOTOR_PORT_LEFT);
	uint16_t currentRIGHT = motor_get_current_MA(MOTOR_PORT_RIGHT);	
	int8_t countL = 0;
	int8_t countR = 0;
	for(int i=0; i< 10 ;i++){
		if(currentLEFT > current_threshold)
			countL ++;	
		if(currentRIGHT > current_threshold)
			countR ++;
	}
	if (countL > 7){
		return 1;
		printf("\nCurrent L %d, R %d", currentLEFT, currentRIGHT);
			
	}else if (countR > 7){
		return -1;
		printf("\nCurrent L %d, R %d", currentLEFT, currentRIGHT);
	
	}else{
		return 0;
	}	
} 

